/**
  ******************************************************************************
  * @file   
  * @brief  
  ******************************************************************************
  * @note

 */

/* Includes ------------------------------------------------------------------*/
#include "integrator_Differentiator.h"
/* Definitions ---------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/

/* Global variables ---------------------------------------------------------*/

/*Functions------------------------------------------------------------------*/

void systick_integrator_init(integrator_op * int_op,float uplimit,float downlimit){
	int_op->last_time_us=osKernelGetSysTimerCount();
	int_op->Integral_value=0;
	int_op->down_limit=downlimit;
	int_op->up_limit=uplimit;
}

void systick_integrator(float input_value,integrator_op * int_op){

  int_op->now_time_us=osKernelGetSysTimerCount();
	if((int_op->now_time_us)>=(int_op->last_time_us)){
	   int_op->now_dt=(int_op->now_time_us-int_op->last_time_us)*System_tick_us/System_tick_division;
	}
	else{
	   int_op->now_dt=(0xffffffff+int_op->now_time_us-int_op->last_time_us)*System_tick_us/System_tick_division;
	}
	if(int_op->Integral_value>int_op->up_limit){
		 int_op->Integral_value=int_op->up_limit;
	}
	else if(int_op->Integral_value<int_op->down_limit){
		 int_op->Integral_value=int_op->down_limit;
	}
	else{
		(int_op->Integral_value)+=(input_value*int_op->now_dt);
	}
	int_op->last_time_us=osKernelGetSysTimerCount();

}

float sgn(float x){
		
	return x/(fabs(x)+0.00001);

}

void differentiator_init(differentiator_op * diff_op,float rate_r,float step_h){
	diff_op->x1_last=0;
	diff_op->x2_last=0;
	diff_op->rate=rate_r;
	diff_op->step=step_h;
}

void fhan(differentiator_op * diff_op ,float value){
	float d=(diff_op->rate)*(diff_op->step);
	float d_0=(diff_op->step)*d;
	float y=(diff_op->x1_last)-value+(diff_op->step)*(diff_op->x2_last);
	float a_0=sqrtf(powf(d,2)+8*(diff_op->rate)*fabs(y));
	float a;
	if(fabs(y)>d_0){
	 a=(diff_op->x2_last)+(a_0-d)/2*sgn(y);
	}
	else{
	 a=(diff_op->x2_last)+y/(diff_op->step);
	}
	if(fabs(a)>d){
		(diff_op->fhan)=-(diff_op->rate)*sgn(a);
	}
	else {
		(diff_op->fhan)=-(diff_op->rate)*a/d;
	}
}

void iteration_differentiator(differentiator_op * diff_op){
		(diff_op->x1_now)=(diff_op->x1_last)+(diff_op->step)*(diff_op->x2_last);
		(diff_op->x2_now)=(diff_op->x2_last)+(diff_op->step)*(diff_op->fhan);
		(diff_op->x1_last)=(diff_op->x1_now);
		(diff_op->x2_last)=(diff_op->x2_now);
}

/* Exported function declarations --------------------------------------------*/


